/*
 * medianFilter.cpp
 *
 *  Created on: 2022年7月14日
 *      Author: john
 */

#if _MSC_VER>=1800
#include "NEON_2_SSE.h"
#else
#include <arm_neon.h>
#endif

#include <string.h>
#include <malloc.h>
#include <omp.h>
#include "myTools.h"
#include "tofalglib.h"

#ifndef vmax_u16
#define vmax_u16(v0, v1, max)\
                        do{\
                            uint16x8_t if_great_than;\
                            if_great_than = vcgtq_u16(v0, v1);\
                            max = vbslq_u16(if_great_than, v0, v1);\
                                                }while(0)
#endif
#ifndef vmin_u16
#define vmin_u16(v0, v1, min)\
                        do{\
                            uint16x8_t if_less_than;\
                            if_less_than = vcltq_u16(v0, v1);\
                            min = vbslq_u16(if_less_than, v0, v1);\
                                                }while(0)
#endif
#ifndef vsort3_max_u16
#define vsort3_max_u16(v0, v1, v2, max)\
                        do{\
                            uint16x8_t if_great_than;\
                            if_great_than = vcgtq_u16(v0, v1);\
                            max = vbslq_u16(if_great_than, v0, v1);\
                            if_great_than = vcgtq_u16(max, v2);\
                            max = vbslq_u16(if_great_than, max, v2);\
                                                                        }while(0)
#endif
#ifndef vsort3_min_u16
#define vsort3_min_u16(v0, v1, v2, min)\
                        do{\
                            uint16x8_t if_less_than;\
                            if_less_than = vcltq_u16(v0, v1);\
                            min = vbslq_u16(if_less_than, v0, v1);\
                            if_less_than = vcltq_u16(min, v2);\
                            min = vbslq_u16(if_less_than, min, v2);\
                                                                        }while(0)
#endif
#ifndef vsort3_med_u16
#define vsort3_med_u16(v0, v1, v2, med)\
                        do{\
                            uint16x8_t if_less_than,if_great_than, max, min;\
                            if_great_than = vcgtq_u16(v0, v1);\
                            max = vbslq_u16(if_great_than, v0, v1);\
                            if_less_than = vcltq_u16(max, v2);\
                            max = vbslq_u16(if_less_than, max, v2);\
                            if_less_than = vcltq_u16(v0, v1);\
                            min = vbslq_u16(if_less_than, v0, v1);\
                            if_great_than = vcgtq_u16(max, min);\
                            med = vbslq_u16(if_great_than, max, min);\
                                                                                                }while(0)
#endif

#ifndef VSORT3INT16
#define VSORT3INT16(v0, v1, v2)\
									do{\
										uint16x8_t min, med, max;\
										vsort3_min_u16(v0, v1, v2, min);\
										vsort3_med_u16(v0, v1, v2, med);\
										vsort3_max_u16(v0, v1, v2, max);\
										v0 = (min);\
										v1 = (med);\
										v2 = (max);\
									}while(0)
#endif

#ifndef VSORT5INT16
#define VSORT5INT16(v0,v1,v2,v3,v4)\
									do{\
										VSORT3INT16(v0, v1, v2); \
										VSORT3INT16(v0, v3, v4); \
										VSORT3INT16(v1, v3, v4); \
										VSORT3INT16(v2, v3, v4);\
									}while(0)

#endif

/*
*   Impliment median 3x3 with arm neon.
*   src_ptr: input buffer
*   dst_ptr: output buffer
*   width  : image width
*   height : image height
*   note: src_ptr and dst_ptr must point to different address.
*/

void median3x3Neon_u16(const unsigned short  *src_ptr, unsigned short *dst_ptr, int width, int height)
{
    unsigned short srcfrsv0, srcfrsv1, srcfrsv2;
    unsigned short srcbksv0, srcbksv1, srcbksv2;

    unsigned short *src_ptr0;
    unsigned short *src_ptr1;
    unsigned short *src_ptr2;
    uint16x8_t *dstv_ptrIn_ = (uint16x8_t *)dst_ptr;
    uint16x8_t src00, src01, src02;
    uint16x8_t src10, src11, src12;
    uint16x8_t src20, src21, src22;
    int k, j;
    uint16x8_t val;
    uint16x8_t v0min, v0med, v0max, v1min, v1med, v1max, v2min, v2med, v2max;
    uint16x8_t v1, v2, v3;

    int startOffset;
    memcpy(dst_ptr, src_ptr, width* 2);
    for (k = 1; k < height - 1; k++)
    {
        startOffset = k*width + 1;
        // 分别指向前中后3行
        src_ptr0 = (unsigned short *)(src_ptr + (k - 1) * width);
        src_ptr1 = (unsigned short *)(src_ptr + (k) * width);
        src_ptr2 = (unsigned short *)(src_ptr + (k + 1) * width);

        // 当前行第一个元素回填
        dst_ptr[k*width] = src_ptr[k*width];
#pragma omp parallel for
        for (j = 0; j < width; j += 8)
        {
            src00 = vld1q_u16((const unsigned short *)(&src_ptr0[j]));
            src01 = vld1q_u16((const unsigned short *)(&src_ptr0[j + 1]));
            src02 = vld1q_u16((const unsigned short *)(&src_ptr0[j + 2]));

            src10 = vld1q_u16((const unsigned short *)(&src_ptr1[j]));
            src11 = vld1q_u16((const unsigned short *)(&src_ptr1[j + 1]));
            src12 = vld1q_u16((const unsigned short *)(&src_ptr1[j + 2]));

            src20 = vld1q_u16((const unsigned short *)(&src_ptr2[j]));
            src21 = vld1q_u16((const unsigned short *)(&src_ptr2[j + 1]));
            src22 = vld1q_u16((const unsigned short *)(&src_ptr2[j + 2]));

            vsort3_min_u16(src00, src01, src02, v0min);
            vsort3_med_u16(src00, src01, src02, v0med);
            vsort3_max_u16(src00, src01, src02, v0max);

            vsort3_min_u16(src10, src11, src12, v1min);
            vsort3_med_u16(src10, src11, src12, v1med);
            vsort3_max_u16(src10, src11, src12, v1max);

            vsort3_min_u16(src20, src21, src22, v2min);
            vsort3_med_u16(src20, src21, src22, v2med);
            vsort3_max_u16(src20, src21, src22, v2max);

            vsort3_max_u16(v0min, v1min, v2min, v1);
            vsort3_med_u16(v0med, v1med, v2med, v2);
            vsort3_min_u16(v0max, v1max, v2max, v3);
            vsort3_med_u16(v1, v2, v3, val);

            vst1q_u16(&dst_ptr[startOffset + j], val);
        }

        // 当前行最后一个元素回填
        dst_ptr[(k + 1)*width - 1] = src_ptr[(k + 1)*width - 1];
    }

    memcpy(&dst_ptr[(height - 1) * width], &src_ptr[(height - 1) * width], width * 2);
}



void test_med()
{
	TINIT;
    int depth_height = 480, depth_width = 640;
    int imSize = depth_height * depth_width;
    unsigned short* depth_ptr_q3 = (unsigned short*)malloc(imSize << 1);
    unsigned short* depth_ptr_q3_ = (unsigned short*)malloc(imSize << 1);
    read_from_raw(depth_ptr_q3, imSize * 2, "/usrdata/test_dsp/median3x3_input.raw");
#if 0
    median3x3Ceva_u16(depth_ptr_q3, depth_ptr_q3_, depth_width, depth_height);
    write_to_raw(depth_ptr_q3_, imSize * 2, "median3x3_output_neon.raw");
#else
//    for(int i = 0; i< 1000; i++)
//    {
		TIC;
		median3x3Neon_u16(depth_ptr_q3, depth_ptr_q3_, depth_width, depth_height);
		TOC("median3x3Neon_u16 cost time:");
//    }
    write_to_raw(depth_ptr_q3_, imSize * 2, "/usrdata/test_dsp/median3x3_output_neon2.raw");
#endif
    free(depth_ptr_q3);
    free(depth_ptr_q3_);
    return;
}


void median5x1CevaOpt(uint16_t *src_ptrIn, uint16_t *dst_ptrIn, int height, int width, int mum)
#ifdef ENABLE_OPTIMISE_DEEP
#pragma func_flags="-O4 -Os0 -OPT:alias=no_typed -OPT:alias=strongly_typed -OPT:alias=restrict -OPT:unroll_times_max=16"
#endif
{
    int calc_count = height * width;

    uint16_t *src_ptr0 = (uint16_t *)src_ptrIn;
    uint16_t *src_ptr1 = (uint16_t *)src_ptrIn + 1 * calc_count;
    uint16_t *src_ptr2 = (uint16_t *)src_ptrIn + 2 * calc_count;
    uint16_t *src_ptr3 = (uint16_t *)src_ptrIn + 3 * calc_count;
    uint16_t *src_ptr4 = (uint16_t *)src_ptrIn + 4 * calc_count;
    //v_uint16_t *dstv_ptrIn = (v_uint16_t *)dst_ptrIn;
    uint16x8_t *dstv_ptrIn_ = (uint16x8_t *)dst_ptrIn;

    uint16x8_t src0, src1, src2, src3, src4;
#pragma omp parallel for
    for (int j = 0; j < calc_count; j += 8)
    {

        src0 = *(uint16x8_t *)&src_ptr0[j];
        src1 = *(uint16x8_t *)&src_ptr1[j];
        src2 = *(uint16x8_t *)&src_ptr2[j];
        src3 = *(uint16x8_t *)&src_ptr3[j];
        src4 = *(uint16x8_t *)&src_ptr4[j];

        VSORT5INT16(src0, src1, src2, src3, src4);

       // *dstv_ptrIn++ = src2;

        vst1q_u16(&dst_ptrIn[j], src2);
    }
}


